#include "src/misc.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <chrono>
using namespace std;
using namespace cv;
using namespace Eigen;

RNG rng(time(0));


struct MinInfo {
    double v;
    int col, row;
    
    friend ostream& operator << (ostream& o, const MinInfo& d) {
        o << "最小值为 " << d.v << endl;
        o << "其位于矩阵的 " << d.row << "行, " << d.col << "列" << endl;
        return o;
    }
};


int main() {
    vector<TGPoint> _pts;
    int n = 2000000;

    //随机生成n个点集，其中前n-1个点构成首尾相连的折线段，最后一个点为定点
    generateRandomPointSet(n, rng, _pts);

    vector<TGPoint>pts;
    pts.reserve(n);
    pts.emplace_back(_pts[0]);
    //去除重复点
    for(int i = 1; i < n; i ++) {
        if(equal(_pts[i], pts.back())) continue;
        pts.emplace_back(_pts[i]);
    }
    n = pts.size();
    
    pair<TGPoint, double>retB;
    retB.second = INT32_MAX;
    //计算离定点最近的折线段距离
    for(int i = 1; i < n-1; i ++) {
        auto tmp = pdist(pts.back(), {pts[i-1], pts[i]});
        //cout << tmp.second << endl;
        if(tmp.second < retB.second) {
            retB = tmp;
        }
    }
    cout << "暴力法点到折线段最近距离为: " << retB.second << endl;


    MatrixXd P(1, 2);
    MatrixXd A(2, pts.size()-1);
    P << pts.back().x, pts.back().y;
    for(int i = 0; i < n-1; i ++) {
        A(0, i) = pts[i].x;
        A(1, i) = pts[i].y;
    }

    //计算定点P到折线段各个端点的最小距离
    auto _P = P.transpose() * MatrixXd::Ones(1, A.cols()); //2*A.cols()
    auto B = _P - A; //2*A.cols()
    // cout << "P: \n" << _P << endl;
    // cout << "A: \n" << A << endl;
    // cout << "P-A: \n" << _P.cwiseProduct(A) << endl;
    auto P2P = B.cwiseProduct(B).colwise().sum().cwiseSqrt(); //点到折线端点的距离
    // cout << "点到直线的距离:\n" << P2P << endl;
    // cout << "点到折线端点的距离\n";
    // cout << P2P << endl;

    // MinInfo m1;
    // m1.v = M1.minCoeff(&m1.row, &m1.col);


    //计算定点到各个折线段垂足的最小距离
    auto A1 = A.block(0, 0, 2, A.cols()-1); //2*A.cols()-1
    auto A2 = A.block(0, 1, 2, A.cols()-1); //2*A.cols()-1
    auto tP = _P.block(0, 0, 2, A.cols()-1);
    auto V1 = tP - A1, V3 = tP - A2;
    auto V2 = A2 - A1;

    auto D1 = V1.cwiseProduct(V2).colwise().sum();
    auto D2 = V3.cwiseProduct(-V2).colwise().sum();

    auto Area = V1.row(0).cwiseProduct(V2.row(1)) - V1.row(1).cwiseProduct(V2.row(0));
    auto _A = A1 - A2;
    auto LineLength = _A.cwiseProduct(_A).colwise().sum().cwiseSqrt();
    auto P2L = Area.cwiseAbs().cwiseProduct(LineLength.cwiseInverse()); //点到直线的距离

    // cout << "Area\n";
    // cout << Area.cwiseAbs() << endl;
    // cout << "LineLength\n";
    // cout << LineLength << endl;
    // cout << "点到折线段距离\n" << P2L << endl;

    cout << ".......\n";
    double retM = P2P.minCoeff();
    auto _q = (D1.array() < 0).select(retM, P2L);
    auto qq = (D2.array() < 0).select(retM, _q);

    // cout << "....Begin For-Loop...\n";
    // for(int i = 0; i < ncol; i ++) {
    //     if(i%100 == 0) printf("%d, ", i);
    //     if(D1(0, i) < 0 || D2(0, i) < 0) continue;
    //     retM = min(retM, P2L(0, i));
    // }

    // cout << "矩阵法点到折线段的最近距离为: " << retM << endl;

    cout << "select: " << qq.minCoeff() << endl;

    return 0;
}